/**
 * @author     B站 李不胖谁胖
 * @version    1.0.0
 * @date       2024-01-20
 */
#include <Adafruit_PWMServoDriver.h>
// Servo shield controller class - assumes default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//控制头俯仰
double servo_0_min_angle = 0;
double servo_0_max_angle = 120;
double servo_0_current_angle = 0;

//控制头旋转
double servo_1_min_angle = 45;
double servo_1_max_angle = 165;
double servo_1_current_angle = 105;

//控制脖子俯仰
double servo_2_min_angle = 0;
double servo_2_max_angle = 50;
double servo_2_current_angle = 0;

//控制左手
double servo_5_min_angle = 20;
double servo_5_max_angle = 100;
double servo_5_current_angle = 100;

//控制右手
double servo_7_min_angle = 100;
double servo_7_max_angle = 180;
double servo_7_current_angle = 100;

// 前进
void handleForward(){
  digitalWrite(6, HIGH);
  digitalWrite(7, LOW);
  digitalWrite(8, HIGH);
  digitalWrite(9, LOW);
}

// 后退
void handleFallback(){
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
  digitalWrite(8, LOW);
  digitalWrite(9, HIGH);
}

// 左转
void handleTurnLeft(){
  digitalWrite(6, HIGH);
  digitalWrite(7, LOW);
  digitalWrite(8, LOW);
  digitalWrite(9, HIGH);
}

// 右转
void handleTurnRight(){
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
  digitalWrite(8, HIGH);
  digitalWrite(9, LOW);
}

// 停止
void handleStop(){
  digitalWrite(6, HIGH);
  digitalWrite(7, HIGH);
  digitalWrite(8, HIGH);
  digitalWrite(9, HIGH);
}

// 控制舵机转动
void slowSetServoPulse(uint8_t n, double step, double current, double min, double max) {
    double min_pulse = getServoPulse(min);
    double max_pulse = getServoPulse(max);
    double current_pulse = getServoPulse(current);
    double target_pulse = getServoPulse(current + step);
    Serial.print("min_pulse:");
    Serial.println(min_pulse);
    Serial.print("max_pulse:");
    Serial.println(max_pulse);
    Serial.print("target_pulse:");
    Serial.println(target_pulse);
    Serial.print("current_pulse:");
    Serial.println(current_pulse);
    if(current_pulse == target_pulse){
      Serial.println("current_pulse == target_pulse:");
       return;
    }

    if(target_pulse > max_pulse || target_pulse < min_pulse){
      Serial.println("outof_pulse:");
       return;
    }
    
    if(current_pulse > target_pulse){
      for(int pulseLength = (int)(current_pulse - 1); pulseLength >= target_pulse ; pulseLength--){
        pwm.setPWM(n, 0, pulseLength);
        //servo_1_current_angle = pulseLength;
        Serial.print("--current_pulse:");
        Serial.println(pulseLength);
        delay(6);
      }
    }
    if(current_pulse < target_pulse){
      for(int pulseLength = (int)(current_pulse + 1); pulseLength <= target_pulse ; pulseLength++){
        pwm.setPWM(n, 0, pulseLength);
        //servo_1_current_angle = pulseLength;
        Serial.print("++current_pulse:");
        Serial.println(pulseLength);
        delay(6);
      }
    }
       Serial.print("servo:");
       Serial.println(n);
       Serial.print("current_angle:");
    if(n==0){
      servo_0_current_angle += step;
       Serial.println(servo_0_current_angle);
    }
    if(n==1){
      servo_1_current_angle += step;
       Serial.println(servo_1_current_angle);
    }
    if(n==2){
      servo_2_current_angle += step;
       Serial.println(servo_2_current_angle);
    }
    if(n==5){
      servo_5_current_angle += step;
       Serial.println(servo_5_current_angle);
    }
    if(n==7){
      servo_7_current_angle += step;
       Serial.println(servo_7_current_angle);
    }
    delay(100);
}

 double getServoPulse(double angle) {
  double pulse = angle;
   pulse = pulse/90 + 0.5;
   double pulselength;
   pulselength = 1000000;   // 1,000,000 us per second
   pulselength /= 50;   // 50 Hz
   pulselength /= 4096;  // 12 bits of resolution
   pulse *= 1000;
   pulse /= pulselength;
   Serial.print("getServoPulse pulse:");
   Serial.println(pulse);
   return pulse;
 }

 //设置9g舵机角度
 void servo_9g_write(uint8_t n,int Angle)
 {
   double pulse = Angle;
   pulse = pulse/90 + 0.5;
   setServoPulse(n,pulse);//0到180度映射为0.5到2.5ms
 }
 
  void setServoPulse(uint8_t n, double pulse) {
   double pulselength;
   pulselength = 1000000;   // 1,000,000 us per second
   pulselength /= 50;   // 50 Hz
   pulselength /= 4096;  // 12 bits of resolution
   pulse *= 1000;
   pulse /= pulselength;
   Serial.print("pulse:");
   Serial.println(pulse);
   pwm.setPWM(n, 0, pulse);
 }

 void demoAction(){
 slowSetServoPulse(0, 90-servo_0_current_angle, servo_0_current_angle,servo_0_min_angle,servo_0_max_angle);
 slowSetServoPulse(1, 75-servo_1_current_angle, servo_1_current_angle,servo_1_min_angle,servo_1_max_angle);
 slowSetServoPulse(5, 50-servo_5_current_angle, servo_5_current_angle,servo_5_min_angle,servo_5_max_angle);
 slowSetServoPulse(7, 100-servo_7_current_angle, servo_7_current_angle,servo_7_min_angle,servo_7_max_angle);
 delay(1000);
 slowSetServoPulse(0, 0-servo_0_current_angle, servo_0_current_angle,servo_0_min_angle,servo_0_max_angle);
 slowSetServoPulse(1, 135-servo_1_current_angle, servo_1_current_angle,servo_1_min_angle,servo_1_max_angle);
 slowSetServoPulse(5, 100-servo_5_current_angle, servo_5_current_angle,servo_5_min_angle,servo_5_max_angle);
 slowSetServoPulse(7, 150-servo_7_current_angle, servo_7_current_angle,servo_7_min_angle,servo_7_max_angle);
 delay(2000);
 slowSetServoPulse(0, 90-servo_0_current_angle, servo_0_current_angle,servo_0_min_angle,servo_0_max_angle);
 slowSetServoPulse(1, 75-servo_1_current_angle, servo_1_current_angle,servo_1_min_angle,servo_1_max_angle);
 slowSetServoPulse(5, 50-servo_5_current_angle, servo_5_current_angle,servo_5_min_angle,servo_5_max_angle);
 slowSetServoPulse(7, 100-servo_7_current_angle, servo_7_current_angle,servo_7_min_angle,servo_7_max_angle);
 delay(1000);
 slowSetServoPulse(0, 0-servo_0_current_angle, servo_0_current_angle,servo_0_min_angle,servo_0_max_angle);
 slowSetServoPulse(1, 135-servo_1_current_angle, servo_1_current_angle,servo_1_min_angle,servo_1_max_angle);
 slowSetServoPulse(5, 100-servo_5_current_angle, servo_5_current_angle,servo_5_min_angle,servo_5_max_angle);
 slowSetServoPulse(7, 150-servo_7_current_angle, servo_7_current_angle,servo_7_min_angle,servo_7_max_angle);
 delay(2000);
}

// 处理动作
void handleMotorAction(char commend){
  if(commend == '\n' || commend == '\r'){
    return;
  }
  Serial.print("commend:");
  Serial.print(commend);
  Serial.println();

  switch (commend){
    case 'w':
      // 如果指令为前进
      handleForward();
        break;
    case 's':
      // 如果指令为后退
      handleFallback();
        break;
    case 'a':
      // 如果指令为左转
      handleTurnLeft();
        break;
    case 'd':
      // 如果指令为右转
      handleTurnRight();
        break;
    case 'q':
      // 如果指令为停止
      handleStop();
        break;
    case 'k':
      // 抬脖子
      slowSetServoPulse(2, -25, servo_2_current_angle,servo_2_min_angle,servo_2_max_angle);
        break; 
    case 'i': 
      // 低脖子
      slowSetServoPulse(2, 25, servo_2_current_angle,servo_2_min_angle,servo_2_max_angle);
        break;
    case 'j':
      // 左转头
      slowSetServoPulse(1, -30, servo_1_current_angle,servo_1_min_angle,servo_1_max_angle);
        break;
    case 'l': 
      // 右转头
      slowSetServoPulse(1, 30, servo_1_current_angle,servo_1_min_angle,servo_1_max_angle);
        break;
    case 'r': 
      // 抬头
      slowSetServoPulse(0, 40, servo_0_current_angle,servo_0_min_angle,servo_0_max_angle);
        break;
    case 'f': 
      // 低头
     slowSetServoPulse(0, -40, servo_0_current_angle,servo_0_min_angle,servo_0_max_angle);
        break;
    case 't':
      // 左手上
      slowSetServoPulse(5, -20, servo_5_current_angle,servo_5_min_angle,servo_5_max_angle);
        break;
    case 'g':
      // 左手下
      slowSetServoPulse(5, 20, servo_5_current_angle,servo_5_min_angle,servo_5_max_angle);
        break;
    case 'y':
      // 右手上
      slowSetServoPulse(7, 20, servo_7_current_angle,servo_7_min_angle,servo_7_max_angle);
        break;
    case 'h':
      // 右手下
      slowSetServoPulse(7, -20, servo_7_current_angle,servo_7_min_angle,servo_7_max_angle);
        break;
    case 'z':
      // 播放音乐4
      //myDFPlayer.playMp3Folder(4);
        break;
    case '1':
      // 播放音乐6
      //myDFPlayer.playMp3Folder(6);
        break;
    case '2':
      // 动作编组1
      demoAction();
        break;
    case '3':
      // 复位
      resetServo();
        break;
    default:
        // 如果不匹配任何指令 不进行操作
        break;
  }
}

void initMotor(){
  // 定义6、7、8、9引脚模式
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  // 初始化时履带为停止状态 所以给6、7、8、9引脚输出高电平
  digitalWrite(6, HIGH);
  digitalWrite(7, HIGH);
  digitalWrite(8, HIGH);
  digitalWrite(9, HIGH);
}

// 关节复位
void resetServo(){
  slowSetServoPulse(0, 0-servo_0_current_angle, servo_0_current_angle,servo_0_min_angle,servo_0_max_angle);
  slowSetServoPulse(1, 105-servo_1_current_angle, servo_1_current_angle,servo_1_min_angle,servo_1_max_angle);
  slowSetServoPulse(2, 0-servo_2_current_angle, servo_2_current_angle,servo_2_min_angle,servo_2_max_angle);
  slowSetServoPulse(5, 100-servo_5_current_angle, servo_5_current_angle,servo_5_min_angle,servo_5_max_angle);
  slowSetServoPulse(7, 100-servo_7_current_angle, servo_7_current_angle,servo_7_min_angle,servo_7_max_angle);
}
void servoSetUp(){
  servo_9g_write(0,0);//控制头俯仰
  servo_9g_write(1,105);//控制头旋转
  servo_9g_write(2,0);//控制脖子俯仰
  // servo_9g_write(3,0);//控制右眼
  // servo_9g_write(4,0);//控制左眼
  servo_9g_write(5,100);//控制左手
  servo_9g_write(7,100);//控制右手
}
void initServo(){
  // Communicate with servo shield (Analog servos run at ~60Hz)
	pwm.begin();
	pwm.setPWMFreq(50);
  servoSetUp();
}

void setup() {
  // put your setup code here, to run once:
  // 初始化串口
  Serial.begin(115200);
  initMotor();
  initServo();
  servoSetUp();
}

void loop() {
  // put your main code here, to run repeatedly:
  // 如果串口接收到了控制指令则读取指令并做出相应的动作
  if (Serial.available() > 0) {
    char commend =  Serial.read();
     handleMotorAction(commend);
  }
}